/*
 * matchTags.cpp
 *
 *  Created on: Nov 16, 2012
 *      Author: Matthijs
 */
#include <camera/camera.h>

using namespace cv;

/**
 *  Match tags.
 */
double Camera::matchTag (tag needle, tag haystack)
{
	//-- Step 3: Matching descriptor vectors using FLANN matcher

	if (!needle.descriptor.data || !haystack.descriptor.data)
		return 100;

	matcher.match( needle.descriptor, haystack.descriptor, matches );
	if (matches.size() == 0)
		return 100;

	double min_dist = 100;
	double distance = 0.0;

	//-- Quick calculation of max and min distances between keypoints
	for( int i = 0; i < matches.size() ; i++ )
	    if( matches[i].distance < min_dist ) min_dist = matches[i].distance;

	//-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )
	std::vector< DMatch > good_matches;

	for( int i = 0; i < matches.size(); i++ )
	{
		if( matches[i].distance < 4 * min_dist ) {
			good_matches.push_back( matches[i]);
			distance += matches[i].distance;
		}
	}
//	ROS_ERROR("Min: %f. Dist: %f (%f). Match count: %d", min_dist, distance, distance / good_matches.size(), good_matches.size());
	return (distance / good_matches.size());
	//return (min_dist < 0.2);


	ROS_INFO("Min dist : %f", min_dist);

	if (min_dist > 0.4)
	{
		ROS_INFO("Insufficient match. Ignoring.");
		return false;
	}

	Mat img_matches = haystack.image;

	//-- Localize the object
	std::vector<Point2f> obj;
	std::vector<Point2f> scene;

	for( int i = 0; i < good_matches.size(); i++ )
	{
		//-- Get the keypoints from the good matches
		obj.push_back( needle.keypoints[ good_matches[i].queryIdx ].pt );
	    scene.push_back( haystack.keypoints[ good_matches[i].trainIdx ].pt );
	}

	Mat H = findHomography( obj, scene, CV_RANSAC );

	//-- Get the corners from the image_1 ( the object to be "detected" )
	std::vector<Point2f> obj_corners(4);
	obj_corners[0] = cvPoint(0,0); obj_corners[1] = cvPoint( needle.image.cols, 0 );
	obj_corners[2] = cvPoint( needle.image.cols, needle.image.rows ); obj_corners[3] = cvPoint( 0, needle.image.rows );
	std::vector<Point2f> scene_corners(4);

	perspectiveTransform( obj_corners, scene_corners, H);



	//-- Draw lines between the corners (the mapped object in the scene - image_2 )
	line( img_matches, scene_corners[0], scene_corners[1], Scalar(0, 255, 0), 4 );
	line( img_matches, scene_corners[1], scene_corners[2], Scalar( 0, 255, 0), 4 );
	line( img_matches, scene_corners[2], scene_corners[3], Scalar( 0, 255, 0), 4 );
	line( img_matches, scene_corners[3], scene_corners[0], Scalar( 0, 255, 0), 4 );

	//-- Show detected matches
//	imshow( "parse window", img_matches );

	for (uint i = 0; i < good_matches.size(); i++ )
		ROS_INFO( "Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d", i, good_matches[i].queryIdx, good_matches[i].trainIdx );

	return true;
}



